﻿#include "kincoservo.h"
#include "global.h"
KincoServo::KincoServo(CanOpenBus *canbus, uint8_t nodeid, uint8_t motortype, QObject *parent)
    : QObject(parent),nodeid(nodeid),type(motortype),myCanbus(canbus)
{

}

bool KincoServo::SetServoParam(int32_t objectindex, int32_t value)
{
    bool result;
    switch (objectindex) {
    case KINCOOBJECTINDEX::RW_WorkMode_int8://工作模式
    {
        // 执行指令并获取执行结果
        result = SetRwWorkMode_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_ControlWord_u16://控制字
    {
        // 执行指令并获取执行结果
        result = SetRwControlWord_SDO(value);
        break;
    }

    case KINCOOBJECTINDEX::RW_TargetPosition_int32://目标位置
    {
        // 执行指令并获取执行结果
        result = SetRwTargetPosition_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_TrapeSpeed_u32://梯形速度
    {
        // 执行指令并获取执行结果
        result = SetRwTrapeSpeed_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_TrapeAcc_u32://梯形加速度
    {
        // 执行指令并获取执行结果
        result = SetRwTrapeAcc_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_TrapeDec_u32://梯形减速度
    {
        // 执行指令并获取执行结果
        result = SetRwTrapeDec_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_TargetSpeed_int32://目标速度
    {
        // 执行指令并获取执行结果
        result = SetRwTargetSpeed_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_TargetTorque_int16://目标扭矩
    {
        // 执行指令并获取执行结果
        result = SetRwTargetTorque_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_EncoderReset_u8://编码器数据复位
    {
        // 执行指令并获取执行结果
        result = SetRwEncoderReset_SDO(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_CanRate_u8://can波特率
    {
        // 执行指令并获取执行结果
        result = SetRwCanRate(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_CanHeartTime_u16://can心跳报文产生时间
    {
        // 执行指令并获取执行结果
        result = SetRwCanHeartTime(value);
        break;
    }
    case KINCOOBJECTINDEX::RW_OriginMode_u8://原点模式
    {
        // 执行指令并获取执行结果
        result = SetRwOriginMode_SDO(value);
        break;
    }

//            case KincoServo:::
//                break;
//            case KincoServo:::
//                break;
    default:
        break;
    }
    return result;
}

void KincoServo::analysisSDOParam(const CanopenSDOFrame &frame)
{
    if(frame.cmdWord == 0x80){
        mylog<<"can sdo object:"<<hex<<frame.objectIndex<<" write/read failed!";
        return;
    }
//    mylog<<frame.NodeID<<" "<<hex<<frame.objectIndex;
    switch (frame.objectIndex) {
    case RO_WorkMode_int8:
        mRO_WorkMode = frame.data;
        break;
    case RO_StateWord_u16:
        mRo_StatusWord = frame.data;
        break;
    case RO_RealPosition_int32:
        mRo_RealPosition = frame.data;
        break;
    case RO_RealSpeed_int32:
        mRo_RealSpeed = SpeedDec2Rpm(frame.data);;
        break;
    case RO_RealCurrent_int16:
        mRo_RealCurrent = CurrentDec2Arms(frame.data);
        break;
    case RO_WarnStateWord_u16:
        mRo_WarnWord = frame.data;
        break;
    case RO_PositiveLimit_int32:
        mRO_PositiveLimit = frame.data;
        break;
    case RO_NagetiveLimit_int32:
        mRO_NagetiveLimit = frame.data;
        break;
    case RO_RealVoltage_u16:
        mRO_RealVoltage = frame.data;
        break;

    case RW_WorkMode_int8:
        mRW_WorkMode = frame.data;
        break;
    case RW_ControlWord_u16:
        mRW_ControlWord = frame.data;
        break;
    case RW_TargetPosition_int32:
        mRW_TargetPosition = frame.data;
        break;
    case RW_TrapeSpeed_u32:
        mRW_TrapeSpeed = SpeedDec2Rpm(frame.data);
        break;
    case RW_TrapeAcc_u32:
        mRW_TrapeAcc = AccDec2Rps(frame.data);
        break;
    case RW_TrapeDec_u32:
        mRW_TrapeDec = AccDec2Rps(frame.data);
        break;
    case RW_TargetSpeed_int32:
        mRW_TargetSpeed = SpeedDec2Rpm(frame.data);
        break;
    case RW_TargetTorque_int16:
        mRW_TargetTorque = frame.data;
        break;
    case RW_EncoderReset_u8:
        mRW_EncoderReset = frame.data;
        break;
    case RW_OriginMode_u8:
        mRW_OriginMode = frame.data;
        break;
    case RW_CanRate_u8:
        mRW_CanRate = frame.data;
        break;
    case RW_CanHeartTime_u16:
        mRW_CanHeartTime = frame.data;
        break;
    case RW_ServoBreak_u8:
        mRW_ServoBreak = frame.data;
        break;
    case RW_DeviceReboot_uint16:
        mRW_DeviceReboot = frame.data;
        break;
    case RW_Save_uint8:
        mRW_Save = frame.data;
        break;
    }
}

/***************************************************************
* 函数名称：SetWorkMOde_SDO 测试通过
* 功能描述：通过SDO设置电机工作模式
* 参数说明：nodeid 从站站号
*   data
*   WorkMode_Pulse = -4;//脉冲模式
    const static int8_t WorkMode_ImmediateSpeed = -3;//立即速度模式
    const static int8_t WorkMode_Location = 1;//位置模式
    const static int8_t WorkMode_Speed = 3;//速度模式
    const static int8_t WorkMode_Torque = 4;//力矩模式
    const static int8_t WorkMode_FindOrigin = 6;//找原点模式
    const static int8_t WorkMode_Runin = 7;//插补模式
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwWorkMode_SDO( int8_t data)
{
    //若参数正确-
    if(data == KINCOWORKMODE::WorkMode_Pulse ||
            data == KINCOWORKMODE::WorkMode_Runin ||
            data == KINCOWORKMODE::WorkMode_Speed ||
            data == KINCOWORKMODE::WorkMode_Torque ||
            data == KINCOWORKMODE::WorkMode_Location ||
            data == KINCOWORKMODE::WorkMode_FindOrigin ||
            data == KINCOWORKMODE::WorkMode_ImmediateSpeed)
    {
        bool ret = myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_WorkMode_int8,1,data);
        if(!ret){
            mylog<<"can write id:"<<nodeid<<" set workmode "<<data<<" failed!";
            return false;
        }
    }
    //参数不正确
    else{
         return false;
    }
}

bool KincoServo::GetRwWorkMode_SDO()
{
    bool ret = myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_WorkMode_int8);
    if(!ret){
        mylog<<"can SDO read id:"<<nodeid<<" workmode failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetControlWord 测试通过
* 功能描述：通过SDO设置电机的控制字
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwControlWord_SDO( uint16_t data)
{
    bool ret = myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_ControlWord_u16,2,data);
    if(!ret){
        mylog<<"can write id:"<<nodeid<<" set ControlWord "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwControlWord_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_ControlWord_u16)){
        mylog<<"can SDO read id:"<<nodeid<<" ControlWord failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetTargetPosition 测试通过
* 功能描述：通过SDO设置电机转动的目标位置
* 参数说明：nodeid 从站站号 data 目标位置（hex）
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwTargetPosition_SDO( int32_t data)
{
    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_TargetPosition_int32,4,data)){
        mylog<<"can SDO write id:"<<nodeid<<" set TargetPosition "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwTargetPosition_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_TargetPosition_int32)){
        mylog<<"can SDO read id:"<<nodeid<<" TargetPosition failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetTrapeSpeed 测试通过从
* 功能描述：通过SDO设置电机转动的梯形速度
* 参数说明：nodeid 从站站号 data 速度（rpm）
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwTrapeSpeed_SDO( uint32_t data)
{
    //将rpm转为十进制编码器实际值
    uint32_t speed = SpeedRpm2Dec(data);
    //限幅

    if(!myCanbus->SDO_Write(nodeid,RW_TrapeSpeed_u32,4,speed)){
        mylog<<"can SDO write id:"<<nodeid<<" set TrapeSpeed "<<speed<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwTrapeSpeed_SDO()
{
    // 如果读成功
//    if(canFrame.isSucceed)
//    {
//        // dec转换rpm
//        uint32_t speed = (canFrame.recvFrame.payload()[7]<<24)|(canFrame.recvFrame.payload()[6]<<16)|(canFrame.recvFrame.payload()[5]<<8)|(canFrame.recvFrame.payload()[4]);
//        mylog<<speed;
//        return SpeedDec2Rpm(speed,type);
//    }
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_TrapeSpeed_u32)){
        mylog<<"can SDO read id:"<<nodeid<<" TrapeSpeed failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetTrapeSpeed 测试通过
* 功能描述：通过SDO设置电机转动的梯形加速度
* 参数说明：nodeid 从站站号 data 速度（rps）
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwTrapeAcc_SDO( uint32_t data)
{
    //将rpm转为十进制编码器实际值
    uint32_t speed = AccRps2Dec(data);
    //限幅

    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_TrapeAcc_u32,4,speed)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_TrapeAcc_u32 "<<speed<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwTrapeAcc_SDO()
{
    // 如果读成功
//    if(canFrame.isSucceed)
//    {
//        // dec转rps
//        uint32_t acc =(canFrame.recvFrame.payload()[7]<<24)|(canFrame.recvFrame.payload()[6]<<16)|(canFrame.recvFrame.payload()[5]<<8)|(canFrame.recvFrame.payload()[4]);
//        mylog<< acc;
//        return AccDec2Rps(acc, type);
//    }
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_TrapeAcc_u32)){
        mylog<<"can SDO read id:"<<nodeid<<" TrapeAcc failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetTrapeSpeed 测试通过
* 功能描述：通过SDO设置电机转动的梯形减速度
* 参数说明：nodeid 从站站号 data 速度（rps）
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwTrapeDec_SDO( uint32_t data)
{
    //将rpm转为十进制编码器实际值
    uint32_t speed = AccRps2Dec(data);
    //限幅

    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_TrapeDec_u32,4,speed)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_TrapeDec_u32 "<<speed<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwTrapeDec_SDO()
{
    // 如果读成功
//    if(canFrame.isSucceed)
//    {
//        // dec转rps
//        uint32_t dec = (canFrame.recvFrame.payload()[7]<<24)|(canFrame.recvFrame.payload()[6]<<16)|(canFrame.recvFrame.payload()[5]<<8)|(canFrame.recvFrame.payload()[4]);
//        return AccDec2Rps(dec, type);
//    }
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_TrapeDec_u32)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_TrapeDec_u32 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetTargetSpeed 测试通过
* 功能描述：通过SDO设置电机的目标速度
* 参数说明：nodeid 从站站号 data 速度（rpm）
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwTargetSpeed_SDO(int32_t data)
{
    //将rpm转为十进制编码器实际值
    int32_t speed = SpeedRpm2Dec(data);
    //限幅

    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_TargetSpeed_int32,4,speed)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_TargetSpeed_int32 "<<speed<<" failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetRwTargetSpeed_SDO
* 功能描述：通过SDO设置电机的目标速度
* 参数说明：nodeid 从站站号
* 返回值说明：电机目标速度 rpm
***************************************************************/
bool KincoServo::GetRwTargetSpeed_SDO()
{
    // 如果读成功
//    if(canFrame.isSucceed)
//    {
//        //dec 转 rpm
//        int32_t speed_dec = (canFrame.recvFrame.payload()[7]<<24)|(canFrame.recvFrame.payload()[6]<<16)|(canFrame.recvFrame.payload()[5]<<8)|(canFrame.recvFrame.payload()[4]);
//        return SpeedDec2Rpm(speed_dec, type);
//    }
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_TargetSpeed_int32)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_TargetSpeed_int32 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetTargetTorque
* 功能描述：通过SDO设置电机的目标扭矩
* 参数说明：nodeid 从站站号 data 扭矩（%）
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwTargetTorque_SDO( int16_t data)
{
    if(data<0 && data >100) return false;

    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_TargetTorque_int16,2,data)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_TargetTorque_int16 "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwTargetTorque_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_TargetTorque_int16)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_TargetTorque_int16 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetEncoderReset
* 功能描述：通过SDO设置电机的通讯编码器数据复位
* 参数说明：nodeid 从站站号 data
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwEncoderReset_SDO( uint8_t data)
{
    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_EncoderReset_u8,1,data)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_EncoderReset_u8 "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwEncoderReset_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_EncoderReset_u8)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_EncoderReset_u8 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetRwCanRate
* 功能描述：通过SDO设置电机的can通讯波特率
* 参数说明：nodeid 从站站号 data 在.h中有相应的晶态变量
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwCanRate( uint8_t data)
{
    //参数正确
    if(data == KINCOCANRATE::CanRate_1M ||
            data == KINCOCANRATE::CanRate_50k ||
            data == KINCOCANRATE::CanRate_125k ||
            data == KINCOCANRATE::CanRate_250K ||
            data == KINCOCANRATE::CanRate_500K)
    {
        if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_CanRate_u8,1,data)){
            mylog<<"can SDO write id:"<<nodeid<<" set RW_CanRate_u8 "<<data<<" failed!";
            return false;
        }
        return true;
    }
    //参数错误
    else{
        return true;
    }
}

bool KincoServo::GetRwCanRate()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_CanRate_u8)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_CanRate_u8 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetRwCanHeartTime 测试通过
* 功能描述：通过SDO设置电机的can通讯心跳报文，用于监控从站是否正常运行
* 参数说明：nodeid 从站站号 data 时间（ms） 0是不发送
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwCanHeartTime( uint16_t data)
{
    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_CanHeartTime_u16,2,data)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_CanHeartTime_u16 "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwCanHeartTime()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_CanHeartTime_u16)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_CanHeartTime_u16 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：SetRwServoBreak 测试通过
* 功能描述：通过SDO设置电机的是否带报闸，主要用于设置垂直电机
* 参数说明：nodeid 从站站号 data:0-不带 1-带 默认0
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::SetRwServoBreak( uint8_t data)
{
    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_ServoBreak_u8,1,data)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_ServoBreak_u8 "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwServoBreak()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_ServoBreak_u8)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_ServoBreak_u8 failed!";
        return false;
    }
    return true;
}

bool KincoServo::SetRwOriginMode_SDO( int8_t data)
{
    if(!myCanbus->SDO_Write(nodeid,KINCOOBJECTINDEX::RW_OriginMode_u8,1,data)){
        mylog<<"can SDO write id:"<<nodeid<<" set RW_OriginMode_u8 "<<data<<" failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRwOriginMode_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RW_OriginMode_u8)){
        mylog<<"can SDO read id:"<<nodeid<<" RW_ServoBreak_u8 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetWorkMode_SDO
* 功能描述：通过SDO获取电机的工作模式
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::GetRoWorkMode_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_WorkMode_int8)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_WorkMode_int8 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetWorkMode_SDO
* 功能描述：通过SDO获取电机的状态字
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::GetRoStateWord_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_StateWord_u16)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_StateWord_u16 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetRealPosition_SDO
* 功能描述：通过SDO获取电机的实际位置
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::GetRoRealPosition_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_RealPosition_int32)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_RealPosition_int32 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetRealSpeed_SDO
* 功能描述：通过SDO获取电机的实际速度，伺服和步进实际速度字节数不一样
* 参数说明：nodeid 从站站号 type-电机类型
* 返回值说明：实际速度(rpm)
***************************************************************/
bool KincoServo::GetRoRealSpeed_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_RealSpeed_int32)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_RealSpeed_int32 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetRealCurrent_SDO
* 功能描述：通过SDO获取电机的实际电流
* 参数说明：nodeid 从站站号
* 返回值说明：电机的实际电流(arms)
***************************************************************/
bool KincoServo::GetRoRealCurrent_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_RealCurrent_int16)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_RealCurrent_int16 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetWarnStateWord_SDO
* 功能描述：通过SDO获取电机的警告状态字
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::GetRoWarnStateWord_SDO()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_WarnStateWord_u16)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_WarnStateWord_u16 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetRoPositiveLimit
* 功能描述：通过SDO获取电机的正限位位置
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::GetRoPositiveLimit()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_PositiveLimit_int32)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_PositiveLimit_int32 failed!";
        return false;
    }
    return true;
}
/***************************************************************
* 函数名称：GetRoNagetiveLimit
* 功能描述：通过SDO获取电机的负限位位置
* 参数说明：nodeid 从站站号
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool KincoServo::GetRoNagetiveLimit()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_NagetiveLimit_int32)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_NagetiveLimit_int32 failed!";
        return false;
    }
    return true;
}

bool KincoServo::GetRoRealVoltage()
{
    if(!myCanbus->SDO_Read(nodeid,KINCOOBJECTINDEX::RO_RealVoltage_u16)){
        mylog<<"can SDO read id:"<<nodeid<<" RO_RealVoltage_u16 failed!";
        return false;
    }
    return true;
}

bool KincoServo::SetMNTStatus( uint8_t cmd)
{
    return myCanbus->MNT_Set(nodeid,cmd);
}
/***************************************************************
* 函数名称：SpeedDec2Rpm
* 功能描述：hex->rpm  由编码器实际值得到rpm,单位换算关系为 DEC=[(rpm*512*编码器分辨率)/1875
* 参数说明：dec-速度的编码器实际值 type-电机类型
* 返回值说明：速度的rpm 转/分
***************************************************************/
float KincoServo::SpeedDec2Rpm(int32_t dec)
{
    if(type == KINCOMOTORTYPE::ServoType)
        return dec/KincoServo::COF_Speed;
    else if(type == KINCOMOTORTYPE::StepType)
        return dec/KincoServo::COF_Speed_Step;
    else
        return 0;
}
/***************************************************************
* 函数名称：SpeedRpm2Dec
* 功能描述：rpm->hex  由rpm得到编码器实际值，单位换算关系为 DEC=[(rpm*512*编码器分辨率)/1875
* 参数说明：速度的rpm 转/分  type-电机类型
* 返回值说明：速度的编码器实际值
***************************************************************/
int32_t KincoServo::SpeedRpm2Dec(int rpm)
{
    if(type == KINCOMOTORTYPE::ServoType)
        return rpm*KincoServo::COF_Speed;
    else if(type == KINCOMOTORTYPE::StepType)
        return rpm*KincoServo::COF_Speed_Step;
    else
        return 0;
}

float KincoServo::AccDec2Rps(uint32_t dec)
{
    if(type == KINCOMOTORTYPE::ServoType)
        return dec/KincoServo::COF_Acc;
    else if(type == KINCOMOTORTYPE::StepType)
        return dec/KincoServo::COF_Acc_Step;
    else
        return 0;
}

uint32_t KincoServo::AccRps2Dec(uint32_t rps)
{
    if(type == KINCOMOTORTYPE::ServoType)
        return rps*KincoServo::COF_Acc;
    else if(type == KINCOMOTORTYPE::StepType)
        return rps*KincoServo::COF_Acc_Step;
    else
        return 0;
}

float KincoServo::CurrentDec2Arms(uint32_t dec)
{
    return dec*KincoServo::COf_Current;
}

uint32_t KincoServo::CurrentArms2Dec(uint32_t arms)
{
    return arms/KincoServo::COf_Current;
}

void KincoServo::ReadRoParams()
{
     GetRoWorkMode_SDO();
     GetRoStateWord_SDO();
     GetRoRealPosition_SDO();
     GetRoRealSpeed_SDO();
     GetRoRealCurrent_SDO();
     GetRoWarnStateWord_SDO();
     GetRoPositiveLimit();
     GetRoNagetiveLimit();
     GetRoRealVoltage();

//    QJsonObject jsonData;

//    jsonData.insert("CmdType","PROBE_RO");
//    jsonData.insert("DeviceNo",nodeid);
//    jsonData.insert("WorkMode", GetRoWorkMode_SDO()); //工作模式
//    jsonData.insert("StateWord", GetRoStateWord_SDO()); //状态字
//    jsonData.insert("RealPosition", GetRoRealPosition_SDO());     //实际位置
//    jsonData.insert("RealSpeed", GetRoRealSpeed_SDO());    //实际速度
//    jsonData.insert("RealCurrent", GetRoRealCurrent_SDO());  //实际电流
//    jsonData.insert("WarnStateWord", GetRoWarnStateWord_SDO());    //警告状态字

}

void KincoServo::ReadRwParams()
{
     GetRwWorkMode_SDO();
     GetRwControlWord_SDO();
     GetRwTargetPosition_SDO();
     GetRwTrapeSpeed_SDO();
     GetRwTrapeAcc_SDO();
     GetRwTrapeDec_SDO();
     GetRwTargetSpeed_SDO();
     GetRwTargetTorque_SDO();
     GetRwEncoderReset_SDO();
     GetRwCanRate();
     GetRwCanHeartTime();
     GetRwServoBreak();
     GetRwOriginMode_SDO();
}
/***************************************************************
* 函数名称：ArriveTargetPosition_PDO
* 功能描述：RPDO1 工作模式-1个字节  目标位置-4个字节   控制字-2个字节
* 参数说明：nodeid-节点号   targetposition-目标位置
* 返回值说明：
***************************************************************/
bool KincoServo::ArriveTargetPosition_PDO( int32_t targetposition,uint16_t controlword)
{
    uint8_t data[8] = {0};
    data[0] = KINCOWORKMODE::WorkMode_Location;
    data[1] = targetposition;
    data[2] = targetposition>>8;
    data[3] = targetposition>>16;
    data[4] = targetposition>>24;
    data[5] = controlword;    //写3f立即根据目标位置移动
    data[6] = controlword>>8;
    data[7] = 0;

    return myCanbus->PDO_Write(nodeid,CANOPENCOBID::RPDO1_COBID,data);
}
/***************************************************************
* 函数名称：ArriveOriginPosition_PDO
* 功能描述：通过PDO修改原点模式，并启动寻找原点
* 参数说明：nodeid-节点号 value-原点模式
* 返回值说明：
***************************************************************/
bool KincoServo::ArriveOriginPosition_PDO()
{
    uint8_t data[8] = {0};
    data[0] = KINCOWORKMODE::WorkMode_FindOrigin;
    data[5] = ControlWord_OringLocationStep2;    //写1f立即进行原点定位
    data[6] = ControlWord_OringLocationStep2>>8;

    return myCanbus->PDO_Write(nodeid,CANOPENCOBID::RPDO1_COBID,data);
}


/***************************************************************
* 函数名称：Frame_SDOProcess
* 功能描述：针对SDO模式的can数据帧进行解析处理
* 参数说明：canframe
* 返回值说明：无
***************************************************************/
void KincoServo::Frame_SDOProcess(QCanBusFrame frameData)
{
    //获取站号
    uint32_t nodeid = frameData.frameId()-0x580;
    //获取数据
    QByteArray recv_data = frameData.payload();
    //获取命令字
    uint8_t commandWord = recv_data[0];
    //获取对象索引
    uint32_t objectIndex = 0x00000000;
    objectIndex = objectIndex | (recv_data[2]<<16);
    objectIndex = objectIndex | (recv_data[1]<<8);
    objectIndex = objectIndex | recv_data[3];

    switch (commandWord) {
    //发送SDO读报文，接受数据为1个字节-0x4F，2个字节-0x4B，4个字节-0x43，数据有误-0x80
//    case 0x4F:
//        if(objectIndex == KincoServo::RO_WorkMode_int8)
//            m_workmode[nodeid-1] = recv_data[4];
//        else
//            mylog<<"recieve data objectindex error:"<<objectIndex;
//        break;
//    case 0x4B:
//        if(objectIndex == KincoServo::RO_StateWord_u16)
//            m_stateword[nodeid-1] = recv_data[4] | (recv_data[5]<<8);
//        else if(objectIndex == KincoServo::RO_RealCurrent_int16)
//            m_realcurrent[nodeid-1] = recv_data[4] | (recv_data[5]<<8);
//        else if(objectIndex == KincoServo::RO_WarnStateWord_u16)
//            m_warnstateword[nodeid-1] = recv_data[4] | (recv_data[5]<<8);
//        else
//            mylog<<"recieve data objectindex error:"<<objectIndex;
//        break;
//    case 0x43:
//        if(objectIndex == KincoServo::RO_RealPosition_int32)
//            m_realposition[nodeid-1] = recv_data[4] | (recv_data[5]<<8) | (recv_data[6]<<16) | (recv_data[7]<<24);
//        else if(objectIndex == KincoServo::RO_RealSpeed_int32)
//            m_realspeed[nodeid-1] = recv_data[4] | (recv_data[5]<<8) | (recv_data[6]<<16) | (recv_data[7]<<24);
//        else
//            mylog<<"recieve data objectindex error:"<<objectIndex;
//        break;
    //发送SDO写报文，写成功0x60，失败0x80
    case 0x60:
        break;
    case 0x80:
        mylog<<"SDO error objectindex:"<<objectIndex<<",error code :"<<recv_data[7]<<recv_data[6]<<recv_data[5]<<recv_data[4];
        break;
    default:
        break;
    }
}
/***************************************************************
* 函数名称：slot_receivedFrame
* 功能描述：can接受数据曹函数
* 参数说明：
* 返回值说明：
***************************************************************/
void KincoServo::slot_receivedFrame(QCanBusFrame frame)
{
    //获取frameid区分帧类型 0x580-SDO回复  0x700-心跳报文 取最高字节判断
    uint32_t type = frame.frameId() & 0xF00;

    switch (type) {
    //解析SDO报文  已改成同步接收方式
    case 0x500:
        //Frame_SDOProcess(frame);
        break;
    //定时心跳报文
    case  0x700:
        break;
    //没有做针对处理的其他报或者报警报文，不做处理直接上传至上位机
    default:
        mylog<<"recv unprocess can frame:"<<frame.frameId()<<frame.payload();//[0]<<frame.payload()[1]<<frame.payload()[2]<<frame.payload()[3]<<frame.payload()[4]<<frame.payload()[5]<<frame.payload()[6]<<frame.payload()[7];
        break;
    }

}
